/******************************************************************************
STRICTLY CONFIDENTIAL (!)
COPYRIGHT BY MAX AND JAN PEJS

File:	Auto.cpp
Date:	01.09.2012
Author:	Max Pejs
******************************************************************************/

#include "Auto.h"

Auto::Auto()
{
	Ta = 0.01f;
	T = 5000;

	saturation = 100;

	ak_1	= 0.f;
	ek		= 0u;

	pedalAccel = 0u;
	pedalBrake = 0u;

	gear = 1;
	engineSpeed = 0;
	vehicleSpeed = 0.f;
}

Auto::~Auto(){}

void Auto::UpdateSpeedDes(UInt8 value)
{
	if (value > 100)
		pedalAccel = 100;
	else
	pedalAccel = value;
}

void Auto::UpdateBrakeDes(UInt8 value)
{
	if (value > 100)
		pedalBrake = 100;
	else
	pedalBrake = value;
}

UInt16 Auto::GetEngineSpeed(void)
{
	return engineSpeed;
}

UInt16 Auto::GetVehicleSpeed()
{
	return (UInt16)vehicleSpeed;
}

UInt8 Auto::get_k_gear()
{
	switch (gear)
	{
		case 1:
			if ( (vehicleSpeed > 35) || engineSpeed > 7000)
		gear = 2;
		break;

		case 2:
		if (vehicleSpeed > 55 || engineSpeed > 7000)
		gear = 3;
		if (vehicleSpeed < 25 || engineSpeed < 2000)
		gear = 1;
		break;

		case 3:
		if (vehicleSpeed > 95 || engineSpeed > 7000)
		gear = 4;
		if (vehicleSpeed < 40 || engineSpeed < 2000)
		gear = 2;
		break;

		case 4:
		if (vehicleSpeed > 125 || engineSpeed > 7000)
		gear = 5;
		if (vehicleSpeed < 60 || engineSpeed < 2000)
		gear = 3;
		break;

		case 5:
		if (vehicleSpeed < 90 || engineSpeed < 2000)
		gear = 4;
		break;
	}

	switch (gear)
	{
		case 1: return 40;
		case 2: return 27;
		case 3: return 16;
		case 4: return 13;
		case 5: return 10;
	}
	return 40;
}


UInt8 Auto::GetGear(void)
{
	if( (pedalAccel == 0) && (vehicleSpeed <= 0) )
		return 0;
	else
		return gear;
}

void Auto::DRIVE(void)
{

	UInt8 accel_OUT = pedalAccel-pedalBrake;

	if(accel_OUT > saturation)
	{
		accel_OUT = saturation;
	}else
	{
		if(accel_OUT < 0)
		{
			accel_OUT = 0;
		}
	}

	UInt16 motor_OUT = accel_OUT * Diagnostic::PS;

	float k_gear = (float)get_k_gear();

	float f_Auto = (motor_OUT * k_gear) / 9.3;

	/* Widerstand */
	float f_luft	= vehicleSpeed * vehicleSpeed;
	float f_g		= Diagnostic::Weight * 20.f;
	/***************/

	float f_result = f_Auto - f_g - f_luft;
	
	float P = (Ta * f_result) / (float)T;

	float ak = ak_1 + P;

	if(ak < 0)
	{
		ak = 0;
	}
	ak_1 =  ak;

	vehicleSpeed = ak;
	engineSpeed = (UInt16)(k_gear * (vehicleSpeed / (2 * 3.14)) * 29);
	
	if(engineSpeed < 800)
	{
		engineSpeed = 800;
	}
}

UInt8 Auto::GetFuel(void)
{
	return 0u;
}